// Fill out your copyright notice in the Description page of Project Settings.
#include "Algorithm/Octree/OctreeNode.h"
#include "Kismet/KismetMathLibrary.h"
#include "Kismet/KismetSystemLibrary.h"
#include "Algorithm/Battery.h"

UObject* OctreeNode::worldObject = nullptr;

bool OctreeNode::IsNotUsed()
{
	return true;
}

bool OctreeNode::InterSection(const FVector OCenter, const float Radian) const
{
	//取相对原点
	const FVector v = OCenter - center;
	float x = UKismetMathLibrary::Min(v.X, extend.X);
	x = UKismetMathLibrary::Max(x, -extend.X);

	float y = UKismetMathLibrary::Min(v.Y, extend.Y);
	y = UKismetMathLibrary::Max(y, -extend.Y);

	float z = UKismetMathLibrary::Min(v.Z, extend.Z);
	z = UKismetMathLibrary::Max(z, -extend.Z);
	// 注意此时圆心的相对坐标
	return (x - v.X) * (x - v.X) + (y - v.Y) * (y - v.Y) + (z - v.Z) * (z - v.Z) <= Radian * Radian * Radian; 
}

bool OctreeNode::InterSection(FVector _point) const
{
	return (_point.X >= center.X - extend.X && _point.X <= center.X + extend.X && _point.Y >= center.Y - extend.Y && _point.Y <= center.Y + extend.Y && _point.Z >= center.Z - extend.Z && _point.Z <= center.Z + extend.Z);
}

int32 OctreeNode::SelectBestChild(FVector _point) const
{
	return (_point.X <= center.X ? 0 : 1) + (_point.Y >= center.Y ? 0 : 4) + (_point.Z <= center.Z ? 0 : 2);
}

void OctreeNode::Split()
{
	const float quarter = extend.X / 2.0f;
	root = root.IsValid() ? root : this->AsShared();
	children.Init(nullptr, 8);
	children[0] = MakeShareable(new OctreeNode(center + FVector(-quarter, quarter, -quarter), extend / 2, depth + 1, root));
	children[1] = MakeShareable(new OctreeNode(center + FVector(quarter, quarter, -quarter), extend / 2, depth + 1, root));
	children[2] = MakeShareable(new OctreeNode(center + FVector(-quarter, quarter, quarter), extend / 2, depth + 1, root));
	children[3] = MakeShareable(new OctreeNode(center + FVector(quarter, quarter, quarter), extend / 2, depth + 1, root));
	children[4] = MakeShareable(new OctreeNode(center + FVector(-quarter, -quarter, -quarter), extend / 2, depth + 1, root));
	children[5] = MakeShareable(new OctreeNode(center + FVector(quarter, -quarter, -quarter), extend / 2, depth + 1, root));
	children[6] = MakeShareable(new OctreeNode(center + FVector(-quarter, -quarter, quarter), extend / 2, depth + 1, root));
	children[7] = MakeShareable(new OctreeNode(center + FVector(quarter, -quarter, quarter), extend / 2, depth + 1, root));
}

void OctreeNode::InsertObject(ABattery* obj)
{
	if (obj == nullptr || !InterSection(obj->GetActorLocation())) return;

	int32 childIndex;
	if (children.Num() == 0)
	{
		if (objs.Num() <= maxCount || extend.X <= miniSize)
		{
			//如果当前节点容纳量未满，则直接添加到该节点
			objs.Add(obj);
			return;
		}
		Split();
		check(children.Num()>0);
		for (int32 i = objs.Num() - 1; i >= 0; i--)
		{
			//ABattery* battery = objs[i];
			childIndex = SelectBestChild(objs[i]->GetActorLocation());
			children[childIndex]->InsertObject(objs[i]);
			objs.Swap(i, objs.Num() - 1);
			objs.Pop();
		}
	}

	childIndex = SelectBestChild(obj->GetActorLocation());
	children[childIndex]->InsertObject(obj);
}

void OctreeNode::Merge()
{
	for (const auto& child : children)
	{
		objs.Append(child->objs);
	}
	children.Empty();
}

bool OctreeNode::CanMerge()
{
	int TotalObjCount = objs.Num();
	if (children.Num() > 0)
	{
		for (const auto& child : children)
		{
			if (child->children.Num() > 0)
			{
				return false;
			}
			TotalObjCount += child->objs.Num();
		}
	}
	return TotalObjCount <= maxCount;
}

bool OctreeNode::RemoveObject(ABattery* obj)
{
	bool bRemove = false;
	for (int32 i = 0; i < objs.Num(); i++)
	{
		if (objs[i] == obj)
		{
			objs.RemoveSwap(obj);
			bRemove = true;
			break;
		}
	}

	if (!bRemove && children.Num() > 0)
	{
		const int32 childIndex = SelectBestChild(obj->GetActorLocation());
		children[childIndex]->RemoveObject(obj);
		bRemove = true;
	}

	if (bRemove && children.Num() > 0 && CanMerge())
	{
		Merge();
	}
	return bRemove;
}

void OctreeNode::DrawBound(float time, float thickness) const
{
	if (worldObject)
	{
		TArray<FLinearColor> colors = {FLinearColor(0.5, 0, 0, 1), FLinearColor(0.5, 0, 0.5, 1), FLinearColor(1, 0.5, 0, 1), FLinearColor(1, 0, 0, 1)};
		const FLinearColor drawColor = bInRange ? FLinearColor::Green : colors[UKismetMathLibrary::Clamp(depth, 0, 3)];
		const FVector drawCenter = center; // +(bInRange ? FVector(0, 0, 8) : FVector(0, 0, 5));
		UKismetSystemLibrary::DrawDebugBox(worldObject, drawCenter, extend, drawColor, FRotator::ZeroRotator, time, thickness + depth * 0.2);
	}
}

void OctreeNode::TraceObjectInRange(AActor* traceActor, float _radian)
{
	const FVector _OCenter = traceActor->GetActorLocation();
	bInRange = false;
	if (InterSection(_OCenter, _radian))
	{
		bInRange = true;
		for (int32 i = objs.Num() - 1; i >= 0; i--)
		{
			for (ABattery* obj : objs)
			{
				{
					const bool bCanActive = FVector::Distance(_OCenter, obj->GetActorLocation()) <= _radian;
					obj->ActiveState(bCanActive, traceActor);
					 if (bCanActive)
					 {
					 	DrawBound(1 / UKismetSystemLibrary::GetFrameCount(),0.5);		
					 }
				}
			}
		}
		if (children.Num() > 0)
		{
			for (const auto& child : children)
			{
				child->TraceObjectInRange(traceActor, _radian);
			}
		}
	} else
	{
		TraceObjectOutRange();
	}
}

void OctreeNode::TraceObjectOutRange()
{
	bInRange = false;
	for (int32 i = objs.Num() - 1; i >= 0; i--)
	{
		objs[i]->ActiveState(false, nullptr);
	}
	for (auto& node : children)
	{
		if (node.IsValid())
		{
			node->TraceObjectOutRange();
		}
	}
}

void OctreeNode::UpdateState()
{
	for (int32 i = objs.Num() - 1; i >= 0; i--)
	{
		if (!InterSection(objs[i]->GetActorLocation()))
		{
			ABattery* obj = objs[i];
			RemoveObject(obj);
			root->InsertObject(obj);
		}
	}
	if (children.Num() > 0)
	{
		if (CanMerge()) Merge(); // 回收合并，进行优化
		for (const auto& child : children)
		{
			child->UpdateState();
		}
	}
	if (depth < 0)
	{
		bInRange = false;
		DrawBound(1 / UKismetSystemLibrary::GetFrameCount(), 1); //根据帧数绘制
	}
}
